
#include "bridge.h"

int Bridge::excess_unknowns() {
    // make sure system is not over or under determined
    int nPoints = points.size();
    int unknown_count = 0;

    Point point;

    for(int i = 0; i < nPoints; i++) {
        point = points[i];
        if(point.getAnchorFull()) {
            unknown_count += 2;
        }
        else if(point.getAnchorHalf()) {
            unknown_count += 1;
        }
    }

    return (unknown_count + members.size()) - 2 * nPoints;
}

std::vector<Point> Bridge::getPoints() {
    return points;
}

void Bridge::solve() {
    if(excess_unknowns() != 0) {
        std::cout << "excess unknowns: " << excess_unknowns()
                  << std::endl;
        return;
    }

    int nMembers = members.size();
    int nPoints = points.size();

    Member member;
    Point point_A, point_1, point_2;

    // build the coefficient matrix
    mat.resize(2 * nPoints, 2 * nPoints);

    for(int i = 0; i < nPoints; i++) {
        point_A = points[i];
        if(point_A.getAnchorFull()) {
            mat(i * 2, nPoints + 0) = 1;
            mat(i * 2 + 1, nPoints + 1) = 1;
        }
        else if(point_A.getAnchorHalf()) {
            mat(i * 2 + 1, nPoints + 2) = 1;
        }

        for(int j = 0; j < nMembers; j++) {
            member = members[j];
            point_1 = member.getPoint1();
            point_2 = member.getPoint2();

            if(point_A.getID() == point_1.getID()) {
                mat(i * 2, j) = member.getRatio1X();
                mat(i * 2 + 1, j) = member.getRatio1Y();
            }
            else if(point_A.getID() == point_2.getID()) {
                mat(i * 2, j) = member.getRatio2X();
                mat(i * 2 + 1, j) = member.getRatio2Y();
            }
        }
    }

    // build the boundary condition vector
    vect.resize(2 * nPoints, 1);
    for(int i = 0; i < nPoints; i++) {
        point_A = points[i];

        vect(2 * i) = -1.0 * point_A.getExtX();
        vect(2 * i + 1) = -1.0 * point_A.getExtY();
    }

    // calc forces
    forces = mat.inverse() * vect;

    // assign forces to each member
    for(int i = 0; i < nMembers; i++) {
        members[i].setForce(forces(i));
    }
}

void Bridge::addMember(Member val) {
    members.push_back(val);
    bool exists1 = false;
    bool exists2 = false;
    int id1 = val.getPoint1().getID();
    int id2 = val.getPoint2().getID();
    int id;

    for(int i = 0; i < points.size(); i++) {
        id = points[i].getID();
        if(id == id1) exists1 = true;
        if(id == id2) exists2 = true;
    }

    if(!exists1) points.push_back(val.getPoint1());
    if(!exists2) points.push_back(val.getPoint2());
}

void Bridge::printResults() {
    for(int i = 0; i < members.size(); i++) {
        std::cout << "member " << i << ": "
                  << members[i].getForce() << std::endl;
    }
}

void Bridge::draw() {
    int nMembers = members.size();
    int nPoints = points.size();

    for(int i = 0; i < nMembers; i++) {
        members[i].draw();
    }
    for(int i = 0; i < nPoints; i++) {
        points[i].draw();
    }
}

